/* 
 * File:   robot.hpp
 * Author: posypkin
 *
 * Created on May 1, 2012, 10:41 AM
 */

#ifndef ROBOT_HPP
#define	ROBOT_HPP

#include <math.h>
#include <problems/cover/common/lipzobjective.hpp>

template <class FT> class RobotObjective : public LipzObjective<FT> {
public:

    struct Coords {
        static const int X = 0;

        static const int Y = 1;
    };

    /**
     * The constructor
     *
     * @param coor coordinate (X or Y)
     * @param nsec (number of sections in the manipulator)
     * @param sections (lengths of sections)
     */
    RobotObjective(int coor, int nsec, FT* sections) :
    mCoor(coor),
    mNSec(nsec),
    mSections(sections),
    mSign(1) {
        FT a = 0;
        for (int i = 0; i < mNSec; i++) {
            FT b = 0;
            for (int j = i; j < mNSec; j++) {
                b += mSections[j];
            }
            a += b * b;
        }
        mLip = sqrt(a);
        printf("lip const= %lf\n", mLip);
    }

    FT func(const FT* x) {
        FT a = 0;
        FT phi = 0;
        for (int i = 0; i < mNSec; i++) {
            FT b;
            phi += x[i];
            if (mCoor == Coords::X)
                b = cos(phi);
            else
                b = sin(phi);
            a += mSections[i] * b;
        }
        return mSign * a;

    }

    FT getFuncLipzConstant(const FT* a, const FT* b) {
        return mLip;
    }

    /**
     * Sets sign (1 for positive, -1 for negative)
     * @param sign
     */
    void setSign(FT sign) {
        mSign = sign;
    }

    int mCoor;
    int mNSec;
    FT *mSections;
    FT mLip;
    FT mSign;
};

#endif	/* ROBOT_HPP */

